// Harris_corner_detection.cpp : 定义控制台应用程序的入口点。
//
//-----------------------------------【头文件包含部分】---------------------------------------  
//      描述：包含程序所依赖的头文件  
//---------------------------------------------------------------------------------------------- 
#include "stdafx.h" 
#include <stdio.h>
#include <iostream>
#include <vector>
#include <corecrt_search.h>
#include <math.h>
#include <opencv2/opencv.hpp>  
#include <opencv2/imgproc/imgproc.hpp>  

//-----------------------------------【命名空间声明部分】---------------------------------------  
//      描述：包含程序所使用的命名空间  
//-----------------------------------------------------------------------------------------------   
using namespace cv;
using namespace std;


struct cr_element
{
    vector<double> element;
	size_t index;
};


double power2(double x, double y)
{
	return((x - y)*(x - y));
}


int cmpTest(const void *pFirst, const void *pSecond)

{

	int nA = *((int *)pFirst);

	int nB = *((int *)pSecond);

	return (nA<nB ? -1 : nA>nB ? 1 : 0); // 由小到大排序
}

double* Cr1(vector<Point2f>corners)
{
	double x[8];
	double y[8];
	double cr[8][6];
	/*提取角点坐标*/
	for (int i = 0; i < 8; i++)
	{
		x[i] = corners[i].x;
		y[i] = corners[i].y;
	}
	/*计算8个角点的cr,共8*6组数据*/
	for (int i = 0; i < 8; i++)
		for (int k = 0, j = 0; (k<6) && (j < 7); j++)
		{
			if (j == i || j == i + 1)
				continue;
			else if(i!=7)
			{
				double down = sqrt(abs((power2(x[i + 1], x[i]) + power2(y[i + 1], y[i]))*(power2(x[j], x[i]) + power2(y[j], y[i]))));
				double cosi = ((x[i + 1] - x[i])*(x[j] - x[i])) / down;
				cr[i][k] = cosi;
				k++;
			}
			if (i == 7)
			{
				cr[i][k] = 0;
				k++;
			}
		}
	/*将每个点的6个cr进行升序排序,然后除max进行归一化*/
	/*归一化*/
	for (int i = 0; i < 8; i++)
		for (int k = 0; k < 6; k++)
		{
			if(cr[i][5]!=0)
			cr[i][k] = cr[i][k] / cr[i][5];
		}
	/*升序*/
	for (int i = 0; i < 8; i++)
	{
		qsort(cr[i], 6, sizeof(double), cmpTest);

	}
	
	/*将cr[i]按每个向量元素的最小值进行升序排列*/
	for (int i = 0; i < 8; i++)
		for (int j = i + 1; j < 8; j++)
		{
			bool flag = (cr[i][1] >= cr[j][1]);
			if (flag)
			{
				for (int k = 0; k < 6; k++)
				{
					int a = cr[i][k];
					cr[i][k] = cr[j][k];
					cr[j][k] = a;
				}
			}
		}
	double * crx=new double[48];
	for (int i = 0; i < 8; i++)
		for (int k = 0; k < 6; k++)
		{
			crx[i * 6 + k] = cr[i][k];
		}
	return crx;
}
/**/
double* Cr2(vector<Point2f>corners)
{
	double x[8];
	double y[8];
	double cr[8][6];
	/*提取角点坐标*/
	for (int i = 0; i < 8; i++)
	{
		x[i] = corners[i].x;
		y[i] = corners[i].y;
	}
	/*计算8个角点的cr,共8*6组数据*/
	for (int i = 0; i < 8; i++)
		for (int k = 0, j = 0; (k<6) && (j < 7); j++)
		{
			if (j == i || j == i + 1)
				continue;
			else if (i != 7)
			{
				double down = sqrt(abs((power2(x[i + 1], x[i]) + power2(y[i + 1], y[i]))*(power2(x[j], x[i]) + power2(y[j], y[i]))));
				double cosi = ((x[i + 1] - x[i])*(x[j] - x[i])) / down;
				cr[i][k] = cosi;
				k++;
			}
			if (i == 7)
			{
				cr[i][k] = 0;
				k++;
			}
		}
	/*将每个点的6个cr进行升序排序,然后除max进行归一化*/
	/*归一化*/
	for (int i = 0; i < 8; i++)
		for (int k = 0; k < 6; k++)
		{
			if (cr[i][5] != 0)
				cr[i][k] = cr[i][k] / cr[i][5];
		}
	/*升序*/
	for (int i = 0; i < 8; i++)
	{
		qsort(cr[i], 6, sizeof(double), cmpTest);
	}

	/*将cr[i]按每个向量元素的最小值进行升序排列*/
	for (int i = 0; i < 8; i++)
		for (int j = i + 1; j < 8; j++)
		{
			bool flag = (cr[i][1] >= cr[j][1]);
			if (flag)
			{
				for (int k = 0; k < 6; k++)
				{
					int a = cr[i][k];
					cr[i][k] = cr[j][k];
					cr[j][k] = a;
				}
			}
		}
	double *crx2 = new double[48];
	for (int i = 0; i < 8; i++)
		for (int k = 0; k < 6; k++)
		{
			crx2[i * 6 + k] = cr[i][k];
		}
	return crx2;
}

vector<Point2f> GetCorners(Mat srcImage)
{
	Mat copy = srcImage;
	vector<Point2f> corners;
	RNG  rng(12345);
	//auto it = corners.begin();
	goodFeaturesToTrack(srcImage, corners, 8, 0.01, 10);
	for (auto it = corners.begin(); it != corners.end(); it++)
	{
		circle(copy, *it, 1, Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 2, 8, 0);
	}

//	imshow("1",copy);
	return corners;
}
void	CompareCr(vector<Point2f>corners1, vector<Point2f>corners2)
{
	double* cr1 = Cr1(corners1);/************?????为什么如果用同一个Cr，这时的crx数组在两次计算中分配的是同一块内存；cr1会跟cr2
												一起变化*************/
	double* cr2 = Cr2(corners2);
	double d=0;
	for (int i = 0; i < 8; i++)
		for (int k = 0; k < 6; k++)
		{
			d += power2(cr1[i*6+k], cr2[i*6+k]);
		}
	d = sqrt(abs(d/8.0));
	if (d <= 0.1)
	{
		printf("same,%f\n",d);
		//return d;

	}
	else
	{
		printf("diff,%f\n",d);
		//return d;
	}
	//delete cr1;
	//delete cr2;
	//return 1.0;
}